//
// Created by pulsarv on 21-1-29.
//
#include <opencv2/opencv.hpp>
#include <rc_system/config.h>
#include <rc_log/slog.hpp>
#include <utility>

namespace rccore {
    namespace common {
        bool Config::Load() {
            cv::FileStorage fsread;
            if (!fsread.open(m_config_file_path, cv::FileStorage::READ)) {
                slog::err << "No such file " << m_config_file_path << " to open" << slog::endl;
                generate_config();
                return false;
            }
            slog::info << "===============" << m_config_file_path
                       << "=================" << slog::endl;
            try {
                //TODO:摄像头配置
                cv::FileNode camera_node = fsread["CameraSettings"];

                pconfigInfo->CAMERA_INDEX = (int) camera_node["CAMERA_INDEX"];
                pconfigInfo->CAMERA_INFO = (std::string) camera_node["CAMERA_INFO"];
                pconfigInfo->RESIZE_WIDTH = (int) camera_node["RESIZE_WIDTH"];
                pconfigInfo->RESIZE_HEIGHT = (int) camera_node["RESIZE_HEIGHT"];
                pconfigInfo->CAMERA_DETCET_SIZE = (float) camera_node["CAMERA_DETCET_SIZE"];
                pconfigInfo->CAMERA_FPS = (int) camera_node["CAMERA_FPS"];

                slog::info << "Camera index:" << pconfigInfo->CAMERA_INDEX << slog::endl;
                slog::info << "Camera Size:" << pconfigInfo->RESIZE_WIDTH
                           << " x " << pconfigInfo->RESIZE_HEIGHT
                           << slog::endl;
                slog::info << "Detcet Size:" << pconfigInfo->CAMERA_DETCET_SIZE << slog::endl;
                slog::info << "Camera FPS:" << pconfigInfo->CAMERA_FPS << slog::endl;
                slog::info << "Camera Param:" << pconfigInfo->CAMERA_INFO << slog::endl;

                //TODO:雷达配置
                cv::FileNode lidar_node = fsread["LidarSettings"];
                pconfigInfo->LIDAR_DEVICE_PATH = (std::string) lidar_node["LIDAR_DEVICE_PATH"];
                slog::info << "Lidar Device Path: " << pconfigInfo->LIDAR_DEVICE_PATH << slog::endl;

                //TODO:IMU配置
                cv::FileNode gyro_node = fsread["GyroSettings"];
                pconfigInfo->GYRO_DEVICE_PATH = (std::string) gyro_node["GYRO_DEVICE_PATH"];
                slog::info << "Gyro Device Path:" << pconfigInfo->GYRO_DEVICE_PATH << slog::endl;
                pconfigInfo->GYRO_DEVICE_FREQ = (int) gyro_node["GYRO_DEVICE_FREQ"];
                slog::info << "Gyro Device Freq:" << pconfigInfo->GYRO_DEVICE_FREQ << slog::endl;
                pconfigInfo->GYRO_DEVICE_HEAD = (int) gyro_node["GYRO_DEVICE_HEAD"];
                slog::info << "Gyro Device Head:" << pconfigInfo->GYRO_DEVICE_HEAD << slog::endl;
                pconfigInfo->GYRO_DEVICE_DATA_SIZE = (int) gyro_node["GYRO_DEVICE_DATA_SIZE"];
                slog::info << "Gyro Device Data Size:" << pconfigInfo->GYRO_DEVICE_DATA_SIZE << slog::endl;

                cv::FileNode moving_control_node = fsread["MovingControlSettings"];
                pconfigInfo->USE_MRAA = (bool) (int) moving_control_node["USE_MRAA"];
                slog::info << "Use MRAA:" << (pconfigInfo->USE_MRAA ? "true" : "false") << slog::endl;
                pconfigInfo->GPIO_1 = (int) moving_control_node["GPIO_1"];
                pconfigInfo->PWM_1 = (int) moving_control_node["PWM_1"];
                pconfigInfo->GPIO_2 = (int) moving_control_node["GPIO_2"];
                pconfigInfo->PWM_2 = (int) moving_control_node["PWM_2"];
                pconfigInfo->GPIO_3 = (int) moving_control_node["GPIO_3"];
                pconfigInfo->PWM_3 = (int) moving_control_node["PWM_3"];

                slog::info << "GPIO: "
                           << pconfigInfo->GPIO_1
                           << "," << pconfigInfo->GPIO_2
                           << "," << pconfigInfo->GPIO_3
                           << slog::endl;

                slog::info << "PWM : "
                           << pconfigInfo->PWM_1
                           << "," << pconfigInfo->PWM_2
                           << "," << pconfigInfo->PWM_3
                           << slog::endl;
                pconfigInfo->DSP_DEVICE_SERIAL_PATH = (std::string) moving_control_node["DSP_DEVICE_SERIAL_PATH"];
                slog::info << "DSP Device Serial Path:" << pconfigInfo->DSP_DEVICE_SERIAL_PATH << slog::endl;
                pconfigInfo->DSP_DEVICE_FREQ = (int) moving_control_node["DSP_DEVICE_FREQ"];
                slog::info << "DSP Device Freq:" << pconfigInfo->DSP_DEVICE_FREQ << slog::endl;

                //TODO:网络模块配置
                cv::FileNode network_node = fsread["NetWorkSettings"];
                pconfigInfo->HTTPD_ON = (bool) (int) network_node["HTTPD_ON"];
                slog::info << "HTTPD_ON:" << (pconfigInfo->HTTPD_ON ? "true" : "false") << slog::endl;
                pconfigInfo->HTTPD_PORT = (int) network_node["HTTPD_PORT"];
                slog::info << "HTTPD_PORT:" << pconfigInfo->HTTPD_PORT << slog::endl;
                pconfigInfo->REMOTE_SERVER_PORT = (int) network_node["REMOTE_SERVER_PORT"];
                slog::info << "REMOTE_SERVER_PORT:" << pconfigInfo->REMOTE_SERVER_PORT << slog::endl;
                pconfigInfo->REMOTE_SERVER_IPADDRESS = (std::string) network_node["REMOTE_SERVER_IPADDRESS"];
                slog::info << "REMOTE_SERVER_IPADDRESS:" << pconfigInfo->REMOTE_SERVER_IPADDRESS << slog::endl;

                //TODO:控制模块配置
                cv::FileNode controller_node = fsread["ControllerSettings"];
                pconfigInfo->DELTA_T = (float) controller_node["DELTA_T"];
                slog::info << "DELTA_T:" << pconfigInfo->DELTA_T << slog::endl;
                controller_node["transitionMatrix"] >> pconfigInfo->transitionMatrix;
//                slog::info << "transitionMatrix: " << pconfigInfo->transitionMatrix << slog::endl;
                pconfigInfo->MAX_POINTS_SIZE = (int) controller_node["MAX_POINTS_SIZE"];
                slog::info << "MAX_POINTS_SIZE: " << pconfigInfo->MAX_POINTS_SIZE << slog::endl;
                pconfigInfo->EKF_ON = (bool) (int) controller_node["EKF_ON"];
                slog::info << "EKF_ON:" << (pconfigInfo->EKF_ON ? "true" : "false") << slog::endl;

                pconfigInfo->WHEEL_1_ON = (bool) (int) controller_node["WHEEL_1_ON"];
                slog::info << "WHEEL_1_ON:" << (pconfigInfo->WHEEL_1_ON ? "true" : "false") << slog::endl;
                pconfigInfo->WHEEL_1_MAX_V_M_S = (float) controller_node["WHEEL_1_MAX_V_M_S"];
                slog::info << "WHEEL_1_MAX_V_M_S:" << pconfigInfo->WHEEL_1_MAX_V_M_S << slog::endl;
                pconfigInfo->WHEEL_1_P = (float) controller_node["WHEEL_1_P"];
                pconfigInfo->WHEEL_1_I = (float) controller_node["WHEEL_1_I"];
                pconfigInfo->WHEEL_1_D = (float) controller_node["WHEEL_1_D"];
                slog::info << "WHEEL_1_P:" <<
                           pconfigInfo->WHEEL_1_P
                           << "," << pconfigInfo->WHEEL_1_I
                           << "," << pconfigInfo->WHEEL_1_D
                           << slog::endl;

                pconfigInfo->WHEEL_2_ON = (bool) (int) controller_node["WHEEL_2_ON"];
                slog::info << "WHEEL_2_ON:" << (pconfigInfo->WHEEL_2_ON ? "true" : "false") << slog::endl;
                pconfigInfo->WHEEL_2_MAX_V_M_S = (float) controller_node["WHEEL_2_MAX_V_M_S"];
                slog::info << "WHEEL_2_MAX_V_M_S:" << pconfigInfo->WHEEL_2_MAX_V_M_S << slog::endl;
                pconfigInfo->WHEEL_2_P = (float) controller_node["WHEEL_2_P"];
                pconfigInfo->WHEEL_2_I = (float) controller_node["WHEEL_2_I"];
                pconfigInfo->WHEEL_2_D = (float) controller_node["WHEEL_2_D"];
                slog::info << "WHEEL_2_P:" <<
                           pconfigInfo->WHEEL_2_P
                           << "," << pconfigInfo->WHEEL_2_I
                           << "," << pconfigInfo->WHEEL_2_D
                           << slog::endl;

                pconfigInfo->WHEEL_3_ON = (bool) (int) controller_node["WHEEL_3_ON"];
                slog::info << "WHEEL_3_ON:" << (pconfigInfo->WHEEL_3_ON ? "true" : "false") << slog::endl;
                pconfigInfo->WHEEL_3_MAX_V_M_S = (float) controller_node["WHEEL_3_MAX_V_M_S"];
                slog::info << "WHEEL_3_MAX_V_M_S:" << pconfigInfo->WHEEL_3_MAX_V_M_S << slog::endl;
                pconfigInfo->WHEEL_3_P = (float) controller_node["WHEEL_3_P"];
                pconfigInfo->WHEEL_3_I = (float) controller_node["WHEEL_3_I"];
                pconfigInfo->WHEEL_3_D = (float) controller_node["WHEEL_3_D"];
                slog::info << "WHEEL_3_P:" <<
                           pconfigInfo->WHEEL_3_P
                           << "," << pconfigInfo->WHEEL_3_I
                           << "," << pconfigInfo->WHEEL_3_D
                           << slog::endl;

                //TODO:系统模块配置
                cv::FileNode system_node = fsread["SystemSettings"];
                pconfigInfo->DEBUG_WINDOW = (bool) (int) system_node["DEBUG_WINDOW"];
                slog::info << "DEBUG_WINDOW:" << (pconfigInfo->DEBUG_WINDOW ? "true" : "false") << slog::endl;
                pconfigInfo->AUTO_CONTROL = (bool) (int) system_node["AUTO_CONTROL"];
                slog::info << "AUTO_CONTROL:" << (pconfigInfo->AUTO_CONTROL ? "true" : "false") << slog::endl;
                pconfigInfo->LOSS_HOVER_TIMEOUT_TIME = (int) system_node["LOSS_HOVER_TIMEOUT_TIME"];
                slog::info << "LOSS_HOVER_TIMEOUT_TIME:" << pconfigInfo->LOSS_HOVER_TIMEOUT_TIME << slog::endl;
                pconfigInfo->USE_DETCET_PLUGUNS = (bool) (int) system_node["USE_DETCET_PLUGUNS"];
                slog::info << "USE_DETCET_PLUGUNS:" << (pconfigInfo->USE_DETCET_PLUGUNS ? "true" : "false")
                           << slog::endl;
                pconfigInfo->DETCET_PLUGUNS_PATH = (std::string) system_node["DETCET_PLUGUNS_PATH"];
                slog::info << "DETCET_PLUGUNS_PATH:" << pconfigInfo->DETCET_PLUGUNS_PATH << slog::endl;


                //TODO:消息模块配置
                cv::FileNode message_node = fsread["MessageSettings"];
                pconfigInfo->HAVE_DSP_SERIAL_MESSAGE = (bool) (int) message_node["HAVE_DSP_SERIAL_MESSAGE"];
                slog::info << "HAVE_DSP_SERIAL_MESSAGE:" << (pconfigInfo->HAVE_DSP_SERIAL_MESSAGE ? "true" : "false")
                           << slog::endl;
                pconfigInfo->DSP_SERIAL_MESSAGE_QUEUE_SIZE = (int) message_node["DSP_SERIAL_MESSAGE_QUEUE_SIZE"];
                slog::info << "DSP_SERIAL_MESSAGE_QUEUE_SIZE:" << pconfigInfo->DSP_SERIAL_MESSAGE_QUEUE_SIZE
                           << slog::endl;

                pconfigInfo->HAVE_NETWORK_MESSAGE = (bool) (int) message_node["HAVE_NETWORK_MESSAGE"];
                slog::info << "HAVE_NETWORK_MESSAGE:" << (pconfigInfo->HAVE_NETWORK_MESSAGE ? "true" : "false")
                           << slog::endl;
                pconfigInfo->NETWORK_MESSAGE_QUEUE_SIZE = (int) message_node["NETWORK_MESSAGE_QUEUE_SIZE"];
                slog::info << "NETWORK_MESSAGE_QUEUE_SIZE:" << pconfigInfo->NETWORK_MESSAGE_QUEUE_SIZE << slog::endl;

                pconfigInfo->HAVE_IMAGE_MESSAGE = (bool) (int) message_node["HAVE_IMAGE_MESSAGE"];
                slog::info << "HAVE_IMAGE_MESSAGE:" << (pconfigInfo->HAVE_IMAGE_MESSAGE ? "true" : "false")
                           << slog::endl;
                pconfigInfo->IMAGE_MESSAGE_QUEUE_SIZE = (int) message_node["IMAGE_MESSAGE_QUEUE_SIZE"];
                slog::info << "IMAGE_MESSAGE_QUEUE_SIZE:" << pconfigInfo->IMAGE_MESSAGE_QUEUE_SIZE << slog::endl;

                pconfigInfo->HAVE_DEPTH_IMAGE_MESSAGE = (bool) (int) message_node["HAVE_DEPTH_IMAGE_MESSAGE"];
                slog::info << "HAVE_DEPTH_IMAGE_MESSAGE:" << (pconfigInfo->HAVE_DEPTH_IMAGE_MESSAGE ? "true" : "false")
                           << slog::endl;
                pconfigInfo->DEPTH_IMAGE_MESSAGE_QUEUE_SIZE = (int) message_node["DEPTH_IMAGE_MESSAGE_QUEUE_SIZE"];
                slog::info << "DEPTH_IMAGE_MESSAGE_QUEUE_SIZE:" << pconfigInfo->DEPTH_IMAGE_MESSAGE_QUEUE_SIZE
                           << slog::endl;

                pconfigInfo->HAVE_MOVE_MESSAGE = (bool) (int) message_node["HAVE_MOVE_MESSAGE"];
                slog::info << "HAVE_MOVE_MESSAGE:" << (pconfigInfo->HAVE_MOVE_MESSAGE ? "true" : "false") << slog::endl;
                pconfigInfo->MOVE_MESSAGE_QUEUE_SIZE = (int) message_node["MOVE_MESSAGE_QUEUE_SIZE"];
                slog::info << "MOVE_MESSAGE_QUEUE_SIZE:" << pconfigInfo->MOVE_MESSAGE_QUEUE_SIZE << slog::endl;

                pconfigInfo->HAVE_RADAR_MESSAGE = (bool) (int) message_node["HAVE_RADAR_MESSAGE"];
                slog::info << "HAVE_RADAR_MESSAGE:" << (pconfigInfo->HAVE_RADAR_MESSAGE ? "true" : "false")
                           << slog::endl;
                pconfigInfo->RADAR_MESSAGE_QUEUE_SIZE = (int) message_node["RADAR_MESSAGE_QUEUE_SIZE"];
                slog::info << "RADAR_MESSAGE_QUEUE_SIZE:" << pconfigInfo->RADAR_MESSAGE_QUEUE_SIZE << slog::endl;

                pconfigInfo->HAVE_IMU_MESSAGE = (bool) (int) message_node["HAVE_IMU_MESSAGE"];
                slog::info << "HAVE_IMU_MESSAGE:" << (pconfigInfo->HAVE_IMU_MESSAGE ? "true" : "false") << slog::endl;
                pconfigInfo->IMU_MESSAGE_QUEUE_SIZE = (int) message_node["IMU_MESSAGE_QUEUE_SIZE"];
                slog::info << "IMU_MESSAGE_QUEUE_SIZE:" << pconfigInfo->IMU_MESSAGE_QUEUE_SIZE << slog::endl;
            } catch (cv::Exception &e) {
                slog::err << e.what() << slog::endl;
                return false;
            } catch (std::exception &e) {
                slog::err << e.what() << slog::endl;
                return false;
            }

            return true;
        }
    } // namespace config
} // namespace
